# from https://blog.csdn.net/qq_43550173/article/details/116273477
# rewrite by Albert Z
# new: heading

import cv2
import mediapipe as mp
import math


class handDetector():
    def __init__(self, mode=False, maxHands=2, modelComplexity=1, detectionCon=0.5, trackCon=0.5):
        self.mode = mode
        self.maxHands = maxHands
        self.modelComplexity = modelComplexity
        self.detectionCon = detectionCon
        self.trackCon = trackCon
        self.mpHands = mp.solutions.hands
        self.hands = self.mpHands.Hands(self.mode, self.maxHands, self.modelComplexity, self.detectionCon, self.trackCon)
        self.mpDraw = mp.solutions.drawing_utils

    def findHands(self, img, draw=True, ):
        imgRGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
        self.results = self.hands.process(imgRGB)

        # print(results.multi_hand_landmarks)
        if self.results.multi_hand_landmarks:
            for handLms in self.results.multi_hand_landmarks:
                if draw:
                    self.mpDraw.draw_landmarks(img, handLms, self.mpHands.HAND_CONNECTIONS)

        return img

    def findPosition(self, img, handNo=0, draw=True):
        lmList = []
        if self.results.multi_hand_landmarks:
            myHand = self.results.multi_hand_landmarks[handNo]
            for id, lm in enumerate(myHand.landmark):
                # print(id, lm)
                # 获取手指关节点
                h, w, c = img.shape
                cx, cy = int(lm.x*w), int(lm.y*h)
                lmList.append([id, cx, cy])
                if draw:
                    cv2.putText(img, str(int(id)), (cx+10, cy+10), cv2.FONT_HERSHEY_PLAIN,
                                1, (0, 0, 255), 2)

        return lmList

    def fingerStatus(self, lmList):
        fingerList = []
        id, originx, originy = lmList[0]
        keypoint_list = [[3, 4], [6, 8], [10, 12], [14, 16], [18, 20]]

        point = keypoint_list[0]
        id, x1, y1 = lmList[point[0]]
        id, x2, y2 = lmList[point[1]]
        if abs(math.hypot(x2-originx, y2-originy) - math.hypot(x1-originx, y1-originy)) > 20:
            fingerList.append(True)
        else:
            fingerList.append(False)

        for point in keypoint_list[1:]:
            id, x1, y1 = lmList[point[0]]
            id, x2, y2 = lmList[point[1]]
            if math.hypot(x2-originx, y2-originy) > math.hypot(x1-originx, y1-originy):
                fingerList.append(True)
            else:
                fingerList.append(False)
        # heading
        heading = 'unknown'
        if fingerList[1]:
            id, x1, y1 = lmList[8]
            id, x2, y2 = lmList[5]
            if abs(y1 - y2) > abs(x1 - x2) * 1.2:
                if y1 < y2:
                    heading = 'up'
                else:
                    heading = 'down'
            elif abs(x1 - x2) > abs(y1 - y2) * 1.2:
                if x1 < x2:
                    heading = 'right'
                else:
                    heading = 'left'
        elif fingerList[0]:
            id, x1, y1 = lmList[4]
            id, x2, y2 = lmList[1]
            if abs(y1 - y2) > abs(x1 - x2):
                if y1 < y2:
                    heading = 'up'
                else:
                    heading = 'down'
            else:
                if x1 < x2:
                    heading = 'right'
                else:
                    heading = 'left'

        return fingerList, heading


def main():
    cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)
    detector = handDetector()
    while True:
        success, img = cap.read()
        img = detector.findHands(img)
        lmList = detector.findPosition(img, draw=False)
        cv2.imshow("image", img)
        if lmList:
            fingerList, heading = detector.fingerStatus(lmList)
            print(fingerList, heading)
        if cv2.waitKey(2) & 0xFF == 27:
            break
    cap.release()


if __name__ == '__main__':
    main()